﻿/*                       NOTICE
 * 
 * This software was produced for the U. S. Government
 * under Contract No. FA8721-10-C-0001, and is
 * subject to the Rights in Noncommercial Computer Software
 * and Noncommercial Computer Software Documentation Clause
 * (DFARS) 252.227-7014 (JUN 1995)
 * 
 * Approved for Public Release: 11-2435
 * © 2011 The MITRE Corporation. All Rights Reserved. 
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 * http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Kinect;
using TUIO;
using CCT.NUI.HandTracking;
using System.Runtime.InteropServices;
using GRIP.Cuda;

namespace GRIP
{
    /// <summary>
    /// KinectAdapter is the GRIP adapter for retrieving full-body skeleton and finger contacts from a user. It uses
    /// Microsoft's Kinect SDK to get the contacts, optionally furthers that data by finding fingers, and converts the
    /// information into a set of publicly accessible TuioContact objects.
    /// </summary>
    public class KinectAdapter : ITuioAdapter
    {
        #region Class Fields

        /// <summary>
        /// The current mode of finger tracking
        /// </summary>
        public FingerMode FingerMode;

        /// <summary>
        /// True to allow "guesses" from the Kinect SDK, where it isn't confident in a joint location
        /// False to only allow confirmed contacts and disable guesses
        /// </summary>
        public bool AllowInferredContacts;

        /// <summary>
        /// The name of the application using this adapter (used for optional TUIO messages)
        /// </summary>
        public string Source
        {
            get;
            set;
        }

        protected ColorAlignmentMode ColorAlignmentMode;
        protected BackgroundSubtractionMode BackgroundSubtractionMode; 
        protected ColorMode ColorMode;
        protected DepthMode DepthMode;

        private int ClosestUser = -1;
        private KinectSensor Sensor;
        private FingerFinder LeftFingerFinder;
        private FingerFinder RightFingerFinder;
        private short[] DepthFrame = new short[640 * 480];
        private byte[] ColorFrame;
        private BufferFilter<TuioContact> filter;
        protected long ContactID = 0;
        protected Dictionary<TuioContactType, TuioContact> contacts = new Dictionary<TuioContactType, TuioContact>();
        private EventHandler<AllFramesReadyEventArgs> KinectEventHandler;

        protected IKinectImageProcessor ImageProcessor;

        /// <summary>
        /// This event is fired whenever the contact data has been updated and a new frame is ready.
        /// </summary>
        public event EventHandler<ContactFrameReadyEventArgs> ContactFrameReady;
        public event EventHandler<UserExitedEventArgs> UserExited;

        #endregion

        #region Public Contacts
        /// <summary>The left hand thumb contact</summary>
        public TuioContact ThumbLeft;

        /// <summary>The left hand index finger contact</summary>
        public TuioContact IndexFingerLeft;

        /// <summary>The left hand middle finger contact</summary>
        public TuioContact MiddleFingerLeft;

        /// <summary>The left hand ring finger contact</summary>
        public TuioContact RingFingerLeft;

        /// <summary>The left hand pinky contact</summary>
        public TuioContact PinkyLeft;

        /// <summary>The right hand thumb contact</summary>
        public TuioContact ThumbRight;

        /// <summary>The right hand index finger contact</summary>
        public TuioContact IndexFingerRight;

        /// <summary>The right hand middle finger contact</summary>
        public TuioContact MiddleFingerRight;

        /// <summary>The right hand ring finger contact</summary>
        public TuioContact RingFingerRight;

        /// <summary>The right hand pinky contact</summary>
        public TuioContact PinkyRight;

        /// <summary>The central hip contact</summary>
        public TuioContact HipCenter;

        /// <summary>The spine (body) contact</summary>
        public TuioContact Spine;

        /// <summary>The central shoulder (chest) contact</summary>
        public TuioContact ShoulderCenter;

        /// <summary>The head contact</summary>
        public TuioContact Head;

        /// <summary>The left shoulder contact</summary>
        public TuioContact ShoulderLeft;

        /// <summary>The left elbow contact</summary>
        public TuioContact ElbowLeft;

        /// <summary>The left wrist contact</summary>
        public TuioContact WristLeft;

        /// <summary>The left hand contact</summary>
        public TuioContact HandLeft;

        /// <summary>The right hsoulder contact</summary>
        public TuioContact ShoulderRight;

        /// <summary>The right elbow contact</summary>
        public TuioContact ElbowRight;

        /// <summary>The right wrist contact</summary>
        public TuioContact WristRight;

        /// <summary>The right hand contact</summary>
        public TuioContact HandRight;

        /// <summary>The left hip contact</summary>
        public TuioContact HipLeft;

        /// <summary>The left knee contact</summary>
        public TuioContact KneeLeft;

        /// <summary>The left ankle contact</summary>
        public TuioContact AnkleLeft;

        /// <summary>The left foot contact</summary>
        public TuioContact FootLeft;

        /// <summary>The right hip contact</summary>
        public TuioContact HipRight;

        /// <summary>The right knee contact</summary>
        public TuioContact KneeRight;

        /// <summary>The right ankle contact</summary>
        public TuioContact AnkleRight;

        /// <summary>The right foot contact</summary>
        public TuioContact FootRight;
        #endregion

        #region Initialization

        /// <summary>
        /// Internal constructor used for inheriting classes, like KinectNetworkAdapter
        /// </summary>
        protected KinectAdapter()
        {
            InitializeContacts();
        }

        /// <summary>
        /// Creates a new adapter with the given Kinect device settings,
        /// and the given finger tracking settings.
        /// </summary>
        /// <param name="kinectSettings">Settings for the Kinect device</param>
        /// <param name="fingerSettings">Settings for finger tracking, if used</param>
        public static KinectAdapter CreateAdapter(KinectSettings kinectSettings, FingerSettings fingerSettings, IKinectImageProcessor imgProc)
        {
            KinectAdapter kinect = new KinectAdapter();
            kinect.ImageProcessor = imgProc;
            KinectSensor sensor = KinectSensor.KinectSensors.FirstOrDefault((KinectSensor pendingSensor) => { return pendingSensor.Status == KinectStatus.Connected; });

            if (sensor == null)
                throw new NotSupportedException("No Kinects are connected and ready");

            kinect.Sensor = sensor;
            kinect.FingerMode = kinectSettings.FingerMode;
            kinect.AllowInferredContacts = kinectSettings.AllowInferredContacts;
            kinect.Source = System.Environment.MachineName + ";" + kinect.Sensor.DeviceConnectionId;
            kinect.filter = new BufferFilter<TuioContact>(kinectSettings.KinectEntryBufferTime, kinectSettings.KinectExitBufferTime);

            sensor.SkeletonStream.Enable();//kinectSettings.SkeletonSmoothingParameters);
            kinect.SetColorMode(kinectSettings.ColorMode);
            kinect.SetDepthMode(kinectSettings.DepthMode);
            kinect.SetBackgroundSubtractionMode(kinectSettings.BackgroundSubtractionMode);
            kinect.SetColorAlignmentMode(kinectSettings.ColorAlignmentMode);

            kinect.LeftFingerFinder = new FingerFinder(kinect, FingerFinder.HandType.Left, fingerSettings);
            kinect.RightFingerFinder = new FingerFinder(kinect, FingerFinder.HandType.Right, fingerSettings);

            kinect.KinectEventHandler = new EventHandler<AllFramesReadyEventArgs>(kinect.FramesReady_DepthColor);
            kinect.Sensor.AllFramesReady += kinect.KinectEventHandler;

            return kinect;
        }

        private void CalculateFixedCutout(short DefaultDepth)
        {
            short[] fakeDepth = new short[307200];
            for (int i = 0; i < 307200; i++)
                fakeDepth[i] = DefaultDepth;
            CalculateFixedCutout(fakeDepth);
        }

        private void CalculateFixedCutout(short[] DepthValues)
        {
            int[] indices = new int[307200];
            ColorImagePoint[] points = new ColorImagePoint[307200];

            bool isRunning = Sensor.IsRunning;
            if (!isRunning)
                Sensor.Start();
            Sensor.MapDepthFrameToColorFrame(DepthImageFormat.Resolution640x480Fps30, DepthValues, ColorImageFormat.RgbResolution640x480Fps30, points);
            if (!isRunning)
                Sensor.Stop();

            for (int i = 0; i < points.Length; i++)
            {
                ColorImagePoint point = points[i];
                int index = point.Y * 640 + point.X;
                if (index < 0 || index > 307199)
                    indices[i] = 0;
                else
                    indices[i] = index;
            }
            ImageProcessor.SetAlignmentMap(indices);
        }

        protected void InitializeContacts()
        {
            contacts.Add(TuioContactType.ThumbLeft, ThumbLeft = new TuioContact(Source, ContactID++, 0, 0, 0, 0, TuioContactType.ThumbLeft));
            contacts.Add(TuioContactType.IndexFingerLeft, IndexFingerLeft = new TuioContact(Source, ContactID++, 1, 0, 0, 0, TuioContactType.IndexFingerLeft));
            contacts.Add(TuioContactType.MiddleFingerLeft, MiddleFingerLeft = new TuioContact(Source, ContactID++, 2, 0, 0, 0, TuioContactType.MiddleFingerLeft));
            contacts.Add(TuioContactType.RingFingerLeft, RingFingerLeft = new TuioContact(Source, ContactID++, 3, 0, 0, 0, TuioContactType.RingFingerLeft));
            contacts.Add(TuioContactType.PinkyLeft, PinkyLeft = new TuioContact(Source, ContactID++, 4, 0, 0, 0, TuioContactType.PinkyLeft));
            contacts.Add(TuioContactType.ThumbRight, ThumbRight = new TuioContact(Source, ContactID++, 5, 0, 0, 0, TuioContactType.ThumbRight));
            contacts.Add(TuioContactType.IndexFingerRight, IndexFingerRight = new TuioContact(Source, ContactID++, 6, 0, 0, 0, TuioContactType.IndexFingerRight));
            contacts.Add(TuioContactType.MiddleFingerRight, MiddleFingerRight = new TuioContact(Source, ContactID++, 7, 0, 0, 0, TuioContactType.MiddleFingerRight));
            contacts.Add(TuioContactType.RingFingerRight, RingFingerRight = new TuioContact(Source, ContactID++, 8, 0, 0, 0, TuioContactType.RingFingerRight));
            contacts.Add(TuioContactType.PinkyRight, PinkyRight = new TuioContact(Source, ContactID++, 9, 0, 0, 0, TuioContactType.PinkyRight));
            contacts.Add(TuioContactType.HipCenter, HipCenter = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HipCenter));
            contacts.Add(TuioContactType.Spine, Spine = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.Spine));
            contacts.Add(TuioContactType.ShoulderCenter, ShoulderCenter = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ShoulderCenter));
            contacts.Add(TuioContactType.Head, Head = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.Head));
            contacts.Add(TuioContactType.ShoulderLeft, ShoulderLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ShoulderLeft));
            contacts.Add(TuioContactType.ElbowLeft, ElbowLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ElbowLeft));
            contacts.Add(TuioContactType.WristLeft, WristLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.WristLeft));
            contacts.Add(TuioContactType.HandLeft, HandLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HandLeft));
            contacts.Add(TuioContactType.ShoulderRight, ShoulderRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ShoulderRight));
            contacts.Add(TuioContactType.ElbowRight, ElbowRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ElbowRight));
            contacts.Add(TuioContactType.WristRight, WristRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.WristRight));
            contacts.Add(TuioContactType.HandRight, HandRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HandRight));
            contacts.Add(TuioContactType.HipLeft, HipLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HipLeft));
            contacts.Add(TuioContactType.KneeLeft, KneeLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.KneeLeft));
            contacts.Add(TuioContactType.AnkleLeft, AnkleLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.AnkleLeft));
            contacts.Add(TuioContactType.FootLeft, FootLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.FootLeft));
            contacts.Add(TuioContactType.HipRight, HipRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HipRight));
            contacts.Add(TuioContactType.KneeRight, KneeRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.KneeRight));
            contacts.Add(TuioContactType.AnkleRight, AnkleRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.AnkleRight));
            contacts.Add(TuioContactType.FootRight, FootRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.FootRight));
        }
        #endregion

        #region Setters
        public void SetColorMode(ColorMode Mode)
        {
            this.ColorMode = Mode;
            bool isRunning = Sensor.IsRunning;
            if(isRunning)
                Sensor.Stop();
            switch (Mode)
            {
                case GRIP.ColorMode.Off:
                    Sensor.ColorStream.Disable();
                    this.ColorFrame = null;
                    break;
                case GRIP.ColorMode.Rgb_1280x960_12FPS:
                    Sensor.ColorStream.Enable(ColorImageFormat.RgbResolution1280x960Fps12);
                    this.ColorFrame = new byte[1280 * 960 * 4];
                    break;
                case GRIP.ColorMode.Rgb_640x480_30FPS:
                    Sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
                    this.ColorFrame = new byte[640 * 480 * 4];
                    break;
                case GRIP.ColorMode.YuvRaw_640x480_15FPS:
                    Sensor.ColorStream.Enable(ColorImageFormat.RawYuvResolution640x480Fps15);
                    this.ColorFrame = new byte[640 * 480 * 2];
                    break;
                case GRIP.ColorMode.YuvToRgb_640x480_15FPS:
                    Sensor.ColorStream.Enable(ColorImageFormat.YuvResolution640x480Fps15);
                    this.ColorFrame = new byte[640 * 480 * 4];
                    break;
            }
            if (isRunning)
                Sensor.Start();
        }

        public void SetDepthMode(DepthMode Mode)
        {
            this.DepthMode = Mode;
            bool isRunning = Sensor.IsRunning;
            if (isRunning)
                Sensor.Stop();
            switch (Mode)
            {
                case GRIP.DepthMode.Off:
                    Sensor.DepthStream.Disable();
                    this.DepthFrame = null;
                    break;
                case GRIP.DepthMode.On_320x240_30FPS:
                    Sensor.DepthStream.Enable(DepthImageFormat.Resolution320x240Fps30);
                    this.DepthFrame = new short[320 * 240];
                    break;
                case GRIP.DepthMode.On_640x480_30FPS:
                    Sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
                    this.DepthFrame = new short[640 * 480];
                    break;
                case GRIP.DepthMode.On_80x60_30FPS:
                    Sensor.DepthStream.Enable(DepthImageFormat.Resolution80x60Fps30);
                    this.DepthFrame = new short[80 * 60];
                    break;
            }
            if (isRunning)
                Sensor.Start();
        }

        public void SetBackgroundSubtractionMode(BackgroundSubtractionMode Mode)
        {
            this.BackgroundSubtractionMode = Mode;
            bool isRunning = Sensor.IsRunning;
            if (isRunning)
                Sensor.Stop();
            switch (Mode)
            {
                case GRIP.BackgroundSubtractionMode.AllPeople:
                    break;
                case GRIP.BackgroundSubtractionMode.AllPeopleWithAlpha:
                    break;
                case GRIP.BackgroundSubtractionMode.ClosestPerson:
                    break;
                case GRIP.BackgroundSubtractionMode.ClosestPersonWithAlpha:
                    break;
                case GRIP.BackgroundSubtractionMode.Off:
                    break;
            }
            if (isRunning)
                Sensor.Start();
        }

        public void SetColorAlignmentMode(ColorAlignmentMode Mode, short FixedDepth = 9600)
        {
            this.ColorAlignmentMode = Mode;
            bool isRunning = Sensor.IsRunning;
            if (isRunning)
                Sensor.Stop();
            switch (Mode)
            {
                case GRIP.ColorAlignmentMode.Off:
                    break;
                case GRIP.ColorAlignmentMode.FixedDepthMap:
                    CalculateFixedCutout(FixedDepth);
                    break;
                case GRIP.ColorAlignmentMode.RealtimeDepthMap:
                    break;
            }
            if (isRunning)
                Sensor.Start();
        }

        public void SetColorAlignmentMode(ColorAlignmentMode Mode, short[] DepthMap)
        {
            this.ColorAlignmentMode = Mode;
            bool isRunning = Sensor.IsRunning;
            if (isRunning)
                Sensor.Stop();
            switch (Mode)
            {
                case GRIP.ColorAlignmentMode.Off:
                    break;
                case GRIP.ColorAlignmentMode.FixedDepthMap:
                    CalculateFixedCutout(DepthMap);
                    break;
                case GRIP.ColorAlignmentMode.RealtimeDepthMap:
                    break;
            }
            if (isRunning)
                Sensor.Start();
        }

        public void SetImageProcessor(IKinectImageProcessor ImageProcessor)
        {
            this.ImageProcessor = ImageProcessor;
        }

        #endregion

        #region Skeleton Processing
        private void FramesReady_DepthColor(object sender, AllFramesReadyEventArgs e)
        {
            using (DepthImageFrame newDepthFrame = e.OpenDepthImageFrame())
            {
                using (ColorImageFrame newColorFrame = e.OpenColorImageFrame())
                {
                    using (SkeletonFrame newSkeletonFrame = e.OpenSkeletonFrame())
                    {
                        if (newDepthFrame == null || newColorFrame == null || newSkeletonFrame == null)
                        {
                            if (ClosestUser != -1)
                            {
                                UserExited(null, new UserExitedEventArgs(ClosestUser));
                                UserExited(null, new UserExitedEventArgs(ClosestUser));
                                UserExited(null, new UserExitedEventArgs(ClosestUser));
                                ClosestUser = -1;
                            }
                            return;
                        }

                        Skeleton[] skeletonData = new Skeleton[newSkeletonFrame.SkeletonArrayLength];
                        newSkeletonFrame.CopySkeletonDataTo(skeletonData);
                        int closestSkel = FindClosestSkeleton(skeletonData);

                        ImageProcessor.HandleNewColorImageFrame(newColorFrame);
                        if(ColorAlignmentMode != GRIP.ColorAlignmentMode.Off)
                            ImageProcessor.AlignColorFrame();
                        ImageProcessor.HandleNewDepthImageFrame(newDepthFrame);
                        if (BackgroundSubtractionMode != GRIP.BackgroundSubtractionMode.Off && closestSkel != -1)
                            ImageProcessor.BackgroundSubtract(closestSkel + 1);
                        ImageProcessor.GetProcessedDepthFrame(DepthFrame);

                        if (closestSkel != -1)
                        {
                            ClosestUser = closestSkel;
                            ProcessSkeletonFrame(skeletonData[closestSkel]);
                        }

                        else if (ClosestUser != -1)
                        {
                            UserExited(null, new UserExitedEventArgs(ClosestUser));
                            UserExited(null, new UserExitedEventArgs(ClosestUser));
                            UserExited(null, new UserExitedEventArgs(ClosestUser));
                            ClosestUser = -1;
                        }
                    }
                }
            }
        }



        private int FindClosestSkeleton(Skeleton[] skeletonData)
        {
            int trackedSkeleton = -1;
            float minDepth = 100000;

            for (int i = 0; i < skeletonData.Length; i++)
            {
                Skeleton s = skeletonData[i];
                if (s.TrackingState == SkeletonTrackingState.Tracked)
                {
                    float valueZ = s.Position.Z;
                    if (valueZ < minDepth && valueZ < 2)
                    {
                        minDepth = valueZ;
                        trackedSkeleton = i;
                    }
                }
            }
            return trackedSkeleton;
        }

        private void getJointPosition(Joint joint, TuioContact contact, List<TuioContact> livingContacts)
        {
            if (joint.TrackingState == JointTrackingState.Tracked || (AllowInferredContacts && joint.TrackingState == JointTrackingState.Inferred))
            {
                DepthImagePoint point = Sensor.MapSkeletonPointToDepth(joint.Position, DepthImageFormat.Resolution640x480Fps30);
                int depthX = Math.Min(Math.Max(point.X, 0), 639);
                int depthY = Math.Min(Math.Max(point.Y, 0), 479);
                int index = (depthY * 640 + depthX);

                int realDepth = DepthFrame[index];
                if(realDepth == 0)
                    return;

                contact.xpos = (float)depthX / 639.0f;
                contact.ypos = (float)depthY / 479.0f;
                contact.zpos = realDepth;

                livingContacts.Add(contact);
            }
        }

        private void ProcessSkeletonFrame(Skeleton skeleton)
        {
            List<TuioContact> livingContacts = new List<TuioContact>(20);

            JointCollection joints = skeleton.Joints;
            getJointPosition(joints[JointType.HandLeft], HandLeft, livingContacts);
            Task leftHandTask = null;
            Task rightHandTask = null;
            if (FingerMode != FingerMode.Off && HandLeft.active)
            {
                leftHandTask = new Task(LeftFingerFinder.processHand);
                leftHandTask.Start();
            }
            getJointPosition(joints[JointType.HandRight], HandRight, livingContacts);
            if (FingerMode != FingerMode.Off && HandRight.active)
            {
                rightHandTask = new Task(RightFingerFinder.processHand);
                rightHandTask.Start();
            }
            getJointPosition(joints[JointType.HipCenter], HipCenter, livingContacts);
            getJointPosition(joints[JointType.Spine], Spine, livingContacts);
            getJointPosition(joints[JointType.ShoulderCenter], ShoulderCenter, livingContacts);
            getJointPosition(joints[JointType.Head], Head, livingContacts);
            getJointPosition(joints[JointType.ShoulderLeft], ShoulderLeft, livingContacts);
            getJointPosition(joints[JointType.ElbowLeft], ElbowLeft, livingContacts);
            getJointPosition(joints[JointType.WristLeft], WristLeft, livingContacts);
            getJointPosition(joints[JointType.ShoulderRight], ShoulderRight, livingContacts);
            getJointPosition(joints[JointType.ElbowRight], ElbowRight, livingContacts);
            getJointPosition(joints[JointType.WristRight], WristRight, livingContacts);
            getJointPosition(joints[JointType.HipLeft], HipLeft, livingContacts);
            getJointPosition(joints[JointType.KneeLeft], KneeLeft, livingContacts);
            getJointPosition(joints[JointType.AnkleLeft], AnkleLeft, livingContacts);
            getJointPosition(joints[JointType.FootLeft], FootLeft, livingContacts);
            getJointPosition(joints[JointType.HipRight], HipRight, livingContacts);
            getJointPosition(joints[JointType.KneeRight], KneeRight, livingContacts);
            getJointPosition(joints[JointType.AnkleRight], AnkleRight, livingContacts);
            getJointPosition(joints[JointType.FootRight], FootRight, livingContacts);

            if (FingerMode == FingerMode.Synchronous)
            {
                if (leftHandTask != null)
                    leftHandTask.Wait();
                if (rightHandTask != null)
                    rightHandTask.Wait();
            }
                
            if (ThumbLeft.active)
                livingContacts.Add(ThumbLeft);
            if (IndexFingerLeft.active)
                livingContacts.Add(IndexFingerLeft);
            if (MiddleFingerLeft.active)
                livingContacts.Add(MiddleFingerLeft);
            if (RingFingerLeft.active)
                livingContacts.Add(RingFingerLeft);
            if (PinkyLeft.active)
                livingContacts.Add(PinkyLeft);
            if (ThumbRight.active)
                livingContacts.Add(ThumbRight);
            if (IndexFingerRight.active)
                livingContacts.Add(IndexFingerRight);
            if (MiddleFingerRight.active)
                livingContacts.Add(MiddleFingerRight);
            if (RingFingerRight.active)
                livingContacts.Add(RingFingerRight);
            if (PinkyRight.active)
                livingContacts.Add(PinkyRight);

            BufferedFrame<TuioContact> holder = filter.ProcessNewFrame(livingContacts);
            foreach (TuioContact contact in holder.NewObjects)
            {
                contact.active = true;
                contact.session_id = ContactID++;
            }
            foreach (TuioContact contact in holder.RemovedObjects)
                contact.active = false;
            if (ContactFrameReady != null)
            {
                ContactFrameReadyEventArgs args = new ContactFrameReadyEventArgs(
                    holder.NewObjects,
                    holder.ExistingObjects,
                    holder.RemovedObjects,
                    holder.InactiveObjects,
                    DateTime.Now,
                    Source);
                ContactFrameReady(this, args);
            }
        }
        #endregion

        #region Getters
        /// <summary>
        /// Returns the collection of each kinect contact, active or not.
        /// </summary>
        /// <returns>A read-only collection of all contacts</returns>
        public IEnumerable<TuioContact> GetContacts()
        {
            return contacts.Values;
        }

        /// <summary>
        /// Returns the Kinect SDK's Runtime object
        /// </summary>
        /// <returns>The Runtime object representing the Kinect device</returns>
        public KinectSensor GetRuntime()
        {
            return Sensor;
        }

        /// <summary>
        /// Returns the last recorded depth frame. It is 640 * 480 * 2, a pixel's true depth can
        /// be accessed by left-shifting the high byte by 8 and logical ORing it with the low byte.
        /// Example: int depth3 = (DepthFrame[6] &lt;&lt; 8) | DepthFrame[5]
        /// </summary>
        /// <returns>The byte array of raw pixel data from the depth camera</returns>
        public short[] GetDepthFrame()
        {
            return DepthFrame;
        }

        /// <summary>
        /// Returns the last recorded color frame.
        /// </summary>
        /// <returns>The color frame</returns>
        public byte[] GetColorFrame()
        {
            return ColorFrame;
        }

        /// <summary>
        /// Returns the point cloud representing the left hand's contour curve.
        /// </summary>
        /// <returns>A point collection describing the left hand outline</returns>
        public Contour GetLeftContour()
        {
            return LeftFingerFinder.GetContour();
        }

        /// <summary>
        /// Returns the point cloud representing the right hand's contour curve.
        /// </summary>
        /// <returns>A point collection describing the right hand outline</returns>
        public Contour GetRightContour()
        {
            return RightFingerFinder.GetContour();
        }

        /// <summary>
        /// Returns the point cloud representing the left hand's convex hull.
        /// </summary>
        /// <returns>A point collection describing the left hand convex hull</returns>
        public ConvexHull GetLeftHull()
        {
            return LeftFingerFinder.GetConvexHull();
        }

        /// <summary>
        /// Returns the point cloud representing the right hand's convex hull.
        /// </summary>
        /// <returns>A point collection describing the right hand convex hull</returns>
        public ConvexHull GetRightHull()
        {
            return RightFingerFinder.GetConvexHull();
        }

        /// <summary>
        /// Returns a string representing this Kinect's unique ID.
        /// </summary>
        /// <returns>The ID of the Kinect</returns>
        public string GetKinectId()
        {
            return Source;// Nui.DeviceConnectionId;
        }

        #endregion

        #region Start and Stop

        public void Start()
        {
            Sensor.Start();
            Sensor.AllFramesReady += KinectEventHandler;
        }

        /// <summary>
        /// Turns off the Kinect's IR camera and unregisters the device.
        /// </summary>
        public Task Shutdown(System.Threading.CancellationToken token)
        {
            Sensor.AllFramesReady -= KinectEventHandler;
            Task task = Task.Factory.StartNew(() =>
            {
                System.Threading.Thread.Sleep(1000);
                Sensor.Stop();
            }, token);
            return task;
        }
        #endregion

        #region Explicit Kinect Interaction
        /*
        unsafe public void ProcessDepthFrame()
        {
            ImageFrame img = Nui.DepthStream.GetNextFrame(0);
            if (img == null)
                return;
            fixed (byte* p = img.Image.Bits)
            {
                short[] frame;
                //if (!UseDepthA)
                    frame = DepthFrame;
                //else
                //    frame = DepthFrameB;

                fixed (short* s = frame)
                {
                    short* newFramePointer = (short*)p;
                    short* depthFramePointer = s;

                    for (int y = 0; y < 307200; y += 640)
                    {
                        for (short x = 639; x > -1; x--)
                            *(depthFramePointer++) = *(newFramePointer + x);
                        newFramePointer += 640;
                    }
                }
               // UseDepthA = !UseDepthA;
            }
        }*/

        /*
        public void ProcessSkeletonFrame()
        {
            JointsCollection joints;
            SkeletonFrame skeletonFrame = Nui.SkeletonEngine.GetNextFrame(0);
            if (skeletonFrame == null)
                return;
            List<TuioContact> livingContacts = new List<TuioContact>(20);

            float minZ = float.MaxValue;
            int minID = -1;
            int id = 0;
            foreach (SkeletonData data in skeletonFrame.Skeletons)
            {
                if (SkeletonTrackingState.Tracked == data.TrackingState)
                {
                    Joint joint = data.Joints[JointID.Spine];
                    float depth = joint.Position.Z;
                    if (depth != 0 && depth < minZ)
                    {
                        minZ = depth;
                        minID = id;
                    }
                }
                id++;
            }

            if (minID > -1)
            {
                SkeletonData data = skeletonFrame.Skeletons[minID];
                joints = data.Joints;
                getJointPosition(joints[JointID.HandLeft], HandLeft, livingContacts);
                Task leftHandTask = null;
                Task rightHandTask = null;
                if (FingerMode != FingerModes.Off && HandLeft.active)
                {
                    leftHandTask = new Task(LeftFingerFinder.processHand);
                    leftHandTask.Start();
                }
                getJointPosition(joints[JointID.HandRight], HandRight, livingContacts);
                if (FingerMode != FingerModes.Off && HandRight.active)
                {
                    rightHandTask = new Task(RightFingerFinder.processHand);
                    rightHandTask.Start();
                }
                getJointPosition(joints[JointID.HipCenter], HipCenter, livingContacts);
                getJointPosition(joints[JointID.Spine], Spine, livingContacts);
                getJointPosition(joints[JointID.ShoulderCenter], ShoulderCenter, livingContacts);
                getJointPosition(joints[JointID.Head], Head, livingContacts);
                getJointPosition(joints[JointID.ShoulderLeft], ShoulderLeft, livingContacts);
                getJointPosition(joints[JointID.ElbowLeft], ElbowLeft, livingContacts);
                getJointPosition(joints[JointID.WristLeft], WristLeft, livingContacts);
                getJointPosition(joints[JointID.ShoulderRight], ShoulderRight, livingContacts);
                getJointPosition(joints[JointID.ElbowRight], ElbowRight, livingContacts);
                getJointPosition(joints[JointID.WristRight], WristRight, livingContacts);
                getJointPosition(joints[JointID.HipLeft], HipLeft, livingContacts);
                getJointPosition(joints[JointID.KneeLeft], KneeLeft, livingContacts);
                getJointPosition(joints[JointID.AnkleLeft], AnkleLeft, livingContacts);
                getJointPosition(joints[JointID.FootLeft], FootLeft, livingContacts);
                getJointPosition(joints[JointID.HipRight], HipRight, livingContacts);
                getJointPosition(joints[JointID.KneeRight], KneeRight, livingContacts);
                getJointPosition(joints[JointID.AnkleRight], AnkleRight, livingContacts);
                getJointPosition(joints[JointID.FootRight], FootRight, livingContacts);

                if (FingerMode == FingerModes.Synchronous)
                {
                    if (leftHandTask != null)
                        leftHandTask.Wait();
                    if (rightHandTask != null)
                        rightHandTask.Wait();
                }

                if (ThumbLeft.active)
                    livingContacts.Add(ThumbLeft);
                if (IndexFingerLeft.active)
                    livingContacts.Add(IndexFingerLeft);
                if (MiddleFingerLeft.active)
                    livingContacts.Add(MiddleFingerLeft);
                if (RingFingerLeft.active)
                    livingContacts.Add(RingFingerLeft);
                if (PinkyLeft.active)
                    livingContacts.Add(PinkyLeft);
                if (ThumbRight.active)
                    livingContacts.Add(ThumbRight);
                if (IndexFingerRight.active)
                    livingContacts.Add(IndexFingerRight);
                if (MiddleFingerRight.active)
                    livingContacts.Add(MiddleFingerRight);
                if (RingFingerRight.active)
                    livingContacts.Add(RingFingerRight);
                if (PinkyRight.active)
                    livingContacts.Add(PinkyRight);
            }

            BufferedFrame<TuioContact> holder = filter.ProcessNewFrame(livingContacts);
            foreach (TuioContact contact in holder.NewObjects)
            {
                contact.active = true;
                contact.session_id = ContactID++;
            }
            foreach (TuioContact contact in holder.RemovedObjects)
                contact.active = false;
            if (ContactFrameReady != null)
            {
                ContactFrameReadyEventArgs args = new ContactFrameReadyEventArgs(
                    holder.NewObjects,
                    holder.ExistingObjects,
                    holder.RemovedObjects,
                    holder.InactiveObjects,
                    DateTime.Now,
                    Source);
                ContactFrameReady(this, args);
            }
        }
         * */
        #endregion
    }
}
